/*
 * @Description: subscribe lidar odom data
 * @Author: fei
 * @Date: 2021-08-08
 */
#include "subscriber/laser_odom_subscriber.hpp"
#include  <Eigen/Dense>

#include "../../third_party/config_file/config_file.hpp"

namespace subscriber_node
{
    LidarOdomSubscriber::LidarOdomSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size)
    :nh_(nh) 
    {
        lidar_odom_subscriber_ = nh_.subscribe(topic_name, buff_size, &LidarOdomSubscriber::laserOdomHandler, this);
    }

    void LidarOdomSubscriber::laserOdomHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
    {
        OdomPoint odom;
        std::string file("/home/khz/ROS/tools_ws/src/subscriber_node/config.cfg");
        ConfigFile config_file(file);
        config_file.ExtractKeys();

        std::vector<int> rpy_transform_vec = config_file.getValueOfKeyAsStdVectorInt("rpy_transform", "1 0 0 0 1 0 0 0 1");

        odom.timestamp = laserOdometry->header.stamp.toSec();
        odom.x = laserOdometry->pose.pose.position.x;
        odom.y = laserOdometry->pose.pose.position.y;
        odom.z = laserOdometry->pose.pose.position.z;

        double roll, pitch, yaw;
        geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
        //TODO:添加配置文件读取旋转矩阵的代码
        Eigen::Matrix3d rpy_transform;
        for (int i = 0, j = 0; i < 3; i++) {
            rpy_transform(i,0) = rpy_transform_vec[j];
            rpy_transform(i,1) = rpy_transform_vec[j+1];
            rpy_transform(i,2) = rpy_transform_vec[j+2];
            j += 3;
        }
        Eigen::Vector3d standCoordinate; //y轴向左，x轴向前，z轴向上
        standCoordinate[0] = geoQuat.x; //roll 旋转轴
        standCoordinate[1] = geoQuat.y; //pitch 旋转轴
        standCoordinate[2] = geoQuat.z; //yaw 旋转轴
        standCoordinate = rpy_transform * standCoordinate;
        //根据论文坐标系的定义获得roll、pitch、yaw
        tf::Matrix3x3(tf::Quaternion(standCoordinate[0], standCoordinate[1], standCoordinate[2], geoQuat.w)).getRPY(roll, pitch, yaw);

        odom.roll = roll;
        odom.pitch = pitch;
        odom.yaw = yaw;

        odom.q_x = geoQuat.x;
        odom.q_y = geoQuat.y;
        odom.q_z = geoQuat.z;
        odom.q_w = geoQuat.w;
    
        new_odom_deque_.push_back(odom);
        std::cout<<"lidar odom data size: "<<new_odom_deque_.size()<<std::endl;
        return;
    }

    void LidarOdomSubscriber::ParseData(std::deque<OdomPoint>& deque_odom_buff)
    {
        if (new_odom_deque_.size() > 0) 
        {
            deque_odom_buff.insert(deque_odom_buff.end(), new_odom_deque_.begin(), new_odom_deque_.end());
            new_odom_deque_.clear();
        }
    }

    int LidarOdomSubscriber::GetLidarOdomSize()
    {
        return new_odom_deque_.size();
    }
}